#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <mosquitto.h>
#include <jsoncpp/json/json.h>
#include <string>
#include <cstring>
#include "json_data.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "pcl_conversions/pcl_conversions.h"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <sstream>
#include <vector>
class SeatMqttBridge : public rclcpp::Node
{
public:
    SeatMqttBridge() : Node("seat_mqtt_bridge")
    {
        // 声明并获取参数
        this->declare_parameter("ros2_send",0);
        this->declare_parameter("mqtt.host", "localhost");
        this->declare_parameter("mqtt.port", 1883);
        this->declare_parameter("mqtt.pubtopic", "seat/position");
        this->declare_parameter("mqtt.subtopic", "seat/position");
        this->declare_parameter("ros.pubtopic", "seat_position");
        this->declare_parameter("ros.subtopic", "seat_position");

        this->declare_parameter("cloud_enable",false);
        this->declare_parameter("ros.subcloud", "subcloud");
        this->declare_parameter("ros.pubcloud", "pubcloud");
        this->declare_parameter("mqtt.pubcloud","pubcloud");
        this->declare_parameter("mqtt.subcloud","subcloud");
        ros2_send = this->get_parameter("ros2_send").as_int();
        mqtt_host_ = this->get_parameter("mqtt.host").as_string();
        mqtt_port_ = this->get_parameter("mqtt.port").as_int();
        mqtt_pubtopic_ = this->get_parameter("mqtt.pubtopic").as_string();
        mqtt_subtopic_ = this->get_parameter("mqtt.subtopic").as_string();
        std::string ros_pubtopic = this->get_parameter("ros.pubtopic").as_string();
        std::string ros_subtopic = this->get_parameter("ros.subtopic").as_string();

        cloud_enable = this->get_parameter("cloud_enable").as_bool();
        std::string ros_subcloud = this->get_parameter("ros.subcloud").as_string();
        std::string ros_pubcloud = this->get_parameter("ros.pubcloud").as_string();
        mqtt_pubcloud_ = this->get_parameter("mqtt.pubcloud").as_string();
        mqtt_subcloud_ = this->get_parameter("mqtt.subcloud").as_string();
        // 初始化MQTT
        mosquitto_lib_init();
        mqtt_client_ = mosquitto_new(nullptr, true, this);
        
        if (!mqtt_client_) {
            RCLCPP_ERROR(this->get_logger(), "Failed to create MQTT client");
            return;
        }

        // 设置MQTT回调
        mosquitto_message_callback_set(mqtt_client_, on_mqtt_message);
         mosquitto_connect_callback_set(mqtt_client_, on_connect);
        mosquitto_disconnect_callback_set(mqtt_client_, on_disconnect);

         // 启动连接线程
        connect_thread_ = std::thread(&SeatMqttBridge::connect_loop, this);
        // // 连接MQTT服务器
        // int rc = mosquitto_connect(mqtt_client_, mqtt_host.c_str(), mqtt_port, 60);
        // RCLCPP_INFO(this->get_logger(),"IP=%s",mqtt_host.c_str());
        // if (rc != 0) {
        //     RCLCPP_ERROR(this->get_logger(), "Could not connect to MQTT broker: %s", mosquitto_strerror(rc));
        //     mosquitto_destroy(mqtt_client_);
        //     return;
        // }
        
        // 启动MQTT循环线程
        int rc = mosquitto_loop_start(mqtt_client_);
        if (rc != 0) {
            RCLCPP_ERROR(this->get_logger(), "Could not start MQTT loop: %s", mosquitto_strerror(rc));
            mosquitto_disconnect(mqtt_client_);
            mosquitto_destroy(mqtt_client_);
            return;
        }
        
        // // 订阅MQTT主题
        // rc = mosquitto_subscribe(mqtt_client_, nullptr, mqtt_subtopic_.c_str(), 0);
        // if (rc != 0) {
        //     RCLCPP_ERROR(this->get_logger(), "Could not subscribe to MQTT topic: %s", mosquitto_strerror(rc));
        // } else {
        //     RCLCPP_INFO(this->get_logger(), "Subscribed to MQTT topic: %s", mqtt_subtopic_.c_str());
        // }

        // 创建ROS订阅者（接收ROS消息并转发到MQTT）
        
            ros_subscriber_ = this->create_subscription<std_msgs::msg::String>(
                ros_subtopic,
                10,
                std::bind(&SeatMqttBridge::ros_message_callback, this, std::placeholders::_1)
            );

            // 创建ROS发布者（将MQTT消息转发到ROS）
            ros_publisher_ = this->create_publisher<std_msgs::msg::String>(
                ros_pubtopic,
                10
            );
        
        if(ros2_send==1)
          timer_ = this->create_wall_timer(std::chrono::seconds(2), std::bind(&SeatMqttBridge::time_publish_message, this));
        RCLCPP_INFO(this->get_logger(), "Seat MQTT bridge initialized. ROS pubtopic: %s, MQTT pubtopic: %s",
                  ros_pubtopic.c_str(), mqtt_pubtopic_.c_str());
        RCLCPP_INFO(this->get_logger(), "Seat MQTT bridge initialized. ROS subtopic: %s, MQTT subtopic: %s",
                  ros_subtopic.c_str(), mqtt_subtopic_.c_str());
        // 订阅ROS点云话题
        if(cloud_enable == true)
            {
            subcloud_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
                ros_subcloud, 10,
                std::bind(&SeatMqttBridge::pointcloud_callback, this, std::placeholders::_1)
            );
        }
    }

    ~SeatMqttBridge()
    {
              // 停止连接线程
        stop_connect_thread_ = true;
        if (connect_thread_.joinable()) {
            connect_thread_.join();
        }
        
        if (mqtt_client_) {
            mosquitto_loop_stop(mqtt_client_, true);
            mosquitto_disconnect(mqtt_client_);
            mosquitto_destroy(mqtt_client_);
        }
        mosquitto_lib_cleanup();
        // if (mqtt_client_) {
        //     mosquitto_loop_stop(mqtt_client_, true);
        //     mosquitto_disconnect(mqtt_client_);
        //     mosquitto_destroy(mqtt_client_);
        // }
        // mosquitto_lib_cleanup();
    }
    void pointcloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    {
         if (!is_connected_) {
            RCLCPP_WARN(this->get_logger(), "Not connected to MQTT broker, cannot publish message");
            return;
        }
        // 转换ROS PointCloud2到PCL点云
        pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
        pcl::fromROSMsg(*msg, pcl_cloud);
        
        // 1. 提取float类型的x, y, z坐标
        // 每个点3个float（x,y,z），每个float4字节
        size_t num_points = pcl_cloud.size();
        std::vector<float> float_data;
        float_data.reserve(num_points * 3);  // 预分配内存

        for (const auto& point : pcl_cloud) {
            float_data.push_back(point.x);
            float_data.push_back(point.y);
            float_data.push_back(point.z);
        }
        // 2. 将float数组转换为字节流（char*）
        // 注意：float的内存布局直接转换为字节，依赖系统字节序（同一架构设备可直接传输）
        const char* byte_data = reinterpret_cast<const char*>(float_data.data());
        size_t data_size = float_data.size() * sizeof(float);

        // 通过Mosquitto发布（非阻塞）
        int ret = mosquitto_publish(
            mqtt_client_, nullptr,
            mqtt_pubcloud_.c_str(),
            data_size,
            byte_data,
            0,  // QoS 0
            false  // 不保留消息
        );
         RCLCPP_INFO(this->get_logger(),"send point cloud point=%d  byte size=%d",num_points,data_size);
       // mqtt_pubcloud_
         if (ret != MOSQ_ERR_SUCCESS) {
            RCLCPP_WARN(this->get_logger(), "Failed to publish MQTT cloud: %s", mosquitto_strerror(ret));
        }
    }
    // 处理从MQTT接收的消息并发布到ROS
    void handle_mqtt_message(const std::string& payload)
    {
        // 验证JSON格式
        // Json::Reader reader;
        // Json::Value root;
        // if (!reader.parse(payload, root)) {
        //     RCLCPP_WARN(this->get_logger(), "Invalid JSON from MQTT: %s", payload.c_str());
        //     return;
        // }

        // 检查必要的字段
        // if (!root.isMember("seatx") || !root.isMember("seaty") || !root.isMember("seatz")) {
        //     RCLCPP_WARN(this->get_logger(), "Missing fields in MQTT JSON message");
        //     return;
        // }

        // 发布到ROS
        auto msg = std_msgs::msg::String();
        msg.data = payload;  // 直接转发JSON字符串
        ros_publisher_->publish(msg);
      //  RCLCPP_INFO(this->get_logger(), "Published MQTT data to ROS: %s", payload.c_str());
    }

private:
    // ROS回调：接收ROS消息并转发到MQTT
    void ros_message_callback(const std_msgs::msg::String::SharedPtr msg)
    {
        // 验证JSON格式
        // Json::Reader reader;
        // Json::Value root;
        // if (!reader.parse(msg->data, root)) {
        //     RCLCPP_WARN(this->get_logger(), "Invalid JSON from ROS: %s", msg->data.c_str());
        //     return;
        // }

        // 检查必要的字段
        // if (!root.isMember("seatx") || !root.isMember("seaty") || !root.isMember("seatz")) {
        //     RCLCPP_WARN(this->get_logger(), "Missing fields in ROS JSON message");
        //     return;
        // }
         if (!is_connected_) {
            RCLCPP_WARN(this->get_logger(), "Not connected to MQTT broker, cannot publish message");
            return;
        }
        // 发布到MQTT
        int rc = mosquitto_publish(
            mqtt_client_,
            nullptr,
            mqtt_pubtopic_.c_str(),
            msg->data.length(),
            msg->data.c_str(),
            0,
            false
        );

        if (rc != 0) {
            RCLCPP_ERROR(this->get_logger(), "Failed to publish to MQTT: %s", mosquitto_strerror(rc));
        } else {
          //  RCLCPP_INFO(this->get_logger(), "Published ROS data to MQTT: %s", msg->data.c_str());
        }
    }
     // 连接成功回调
    static void on_connect(struct mosquitto* mosq, void* obj, int rc)
    {
        if (!obj) return;
        
        SeatMqttBridge* bridge = static_cast<SeatMqttBridge*>(obj);
        
        if (rc == 0) {
            bridge->is_connected_ = true;
            RCLCPP_INFO(bridge->get_logger(), "Successfully connected to MQTT broker");
            // 连接成功后订阅主题
            bridge->resubscribe_topics();
        } else {
            bridge->is_connected_ = false;
            RCLCPP_ERROR(bridge->get_logger(), "Failed to connect to MQTT broker: %s", mosquitto_strerror(rc));
        }
    }
    // 断开连接回调
    static void on_disconnect(struct mosquitto* mosq, void* obj, int rc)
    {
        if (!obj) return;
        
        SeatMqttBridge* bridge = static_cast<SeatMqttBridge*>(obj);
        
        if (rc != 0) { // 非自愿断开连接
            bridge->is_connected_ = false;
            RCLCPP_WARN(bridge->get_logger(), "Disconnected from MQTT broker. Will attempt to reconnect...");
        }
    }  
    // MQTT消息回调函数（静态方法）
    static void on_mqtt_message(struct mosquitto* mosq, void* obj, const struct mosquitto_message* msg)
    {
        if (!obj || !msg->payload) return;

        SeatMqttBridge* bridge = static_cast<SeatMqttBridge*>(obj);
        std::string payload(static_cast<char*>(msg->payload), msg->payloadlen);
        
        bridge->handle_mqtt_message(payload);
    }
    // 连接循环，处理重连逻辑
    void connect_loop()
    {
        int retry_count = 0;
        const int max_retry_delay = 5; // 最大重连延迟（秒）
        
        while (rclcpp::ok() && !stop_connect_thread_) {
            if (!is_connected_) {
                RCLCPP_INFO(this->get_logger(), "Trying to connect to MQTT broker: %s:%d", 
                           mqtt_host_.c_str(), mqtt_port_);
                
                int rc = mosquitto_connect(mqtt_client_, mqtt_host_.c_str(), mqtt_port_, 60);
                if (rc != 0) {
                    int delay = std::min(1 << retry_count, max_retry_delay); // 指数退避
                    RCLCPP_ERROR(this->get_logger(), "Could not connect to MQTT broker: %s. Retrying in %d seconds...",
                                mosquitto_strerror(rc), delay);
                    
                    // 等待重试
                    std::this_thread::sleep_for(std::chrono::seconds(delay));
                    retry_count++;
                } else {
                    // 连接请求已发送，等待回调确认
                    std::this_thread::sleep_for(std::chrono::seconds(1));
                }
            } else {
                // 已连接，短暂休眠
                std::this_thread::sleep_for(std::chrono::seconds(1));
            }
        }
    }
        // 重新订阅主题
    void resubscribe_topics()
    {
        int rc = mosquitto_subscribe(mqtt_client_, nullptr, mqtt_subtopic_.c_str(), 0);
        if (rc != 0) {
            RCLCPP_ERROR(this->get_logger(), "Could not subscribe to MQTT topic: %s", mosquitto_strerror(rc));
        } else {
            RCLCPP_INFO(this->get_logger(), "Re-subscribed to MQTT topic: %s", mqtt_subtopic_.c_str());
        }
    }
    void time_publish_message(void)  ///测试用
    {
      count++;
      msg_send.seatx=1.1+count;
      msg_send.seaty=1.2;
      msg_send.seatz=1.3;
      msg_send.name = "hello";
      msg_send.set_json_msg();
        // 发布到MQTT
        int rc = mosquitto_publish(
            mqtt_client_,
            nullptr,
            mqtt_pubtopic_.c_str(),
            msg_send.json_msg.length(),
            msg_send.json_msg.c_str(),
            0,
            false
        );
    }
    struct mosquitto* mqtt_client_;
    std::string mqtt_pubtopic_;
    std::string mqtt_subtopic_;
    std::string mqtt_pubcloud_;
    std::string mqtt_subcloud_;
    rclcpp::TimerBase::SharedPtr timer_;
    // ROS订阅器
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subcloud_;  //点云订阅
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr ros_subscriber_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr ros_publisher_;
    JsonData_convert msg_rec;
    JsonData_convert msg_send;
    int ros2_send;
    int count=0;
    bool is_connected_;
    std::thread connect_thread_;
    bool stop_connect_thread_ = false;
    std::string mqtt_host_;
    int mqtt_port_;
    bool cloud_enable;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<SeatMqttBridge>());
    rclcpp::shutdown();
    return 0;
}
    
